{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c1ede8c3",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "import h5py\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib.image as mpimg\n",
    "import os\n",
    "import os.path as osp\n",
    "import imageio.v2 as imageio\n",
    "\n",
    "from scipy.stats import describe"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "240135e5",
   "metadata": {},
   "outputs": [],
   "source": [
    "import re\n",
    "import cv2\n",
    "def imread_cv2(path, options=cv2.IMREAD_COLOR):\n",
    "    \"\"\"Open an image or a depthmap with opencv-python.\"\"\"\n",
    "    if path.endswith((\".exr\", \"EXR\")):\n",
    "        options = cv2.IMREAD_ANYDEPTH\n",
    "    img = cv2.imread(path, flags=options)\n",
    "    if img is None:\n",
    "        raise IOError(f\"Could not load image={path} with {options=}\")\n",
    "    if img.ndim == 3:\n",
    "        img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)\n",
    "    return img"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5adda951",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "# Corrected code for defining matrices A and B\n",
    "A = np.array([\n",
    "    [0.53606341, 0.2906835, -0.79255228, 4.93817743],\n",
    "    [0.80665921, 0.10039632, 0.58242724, 1.55438642],\n",
    "    [0.24887132, -0.95153754, -0.18066373, 1.49607857],\n",
    "    [0., 0., 0., 1.]\n",
    "])\n",
    "\n",
    "B = np.array([\n",
    "    [0.80665921, -0.10039632, -0.58242724, 1.55438642],\n",
    "    [0.53606341, -0.2906835, 0.79255228, 4.93817743],\n",
    "    [-0.24887132, -0.95153754, -0.18066373, -1.49607857],\n",
    "    [0., 0., 0., 1.]\n",
    "])\n",
    "\n",
    "T = np.array([\n",
    "    [0., 1., 0., 0.],\n",
    "    [1., 0., 0., 0.],\n",
    "    [0., 0., -1., 0.],\n",
    "    [0., 0., 0., 1.]\n",
    "])\n",
    "# To calculate T1 and T2, we can use the inverse of B\n",
    "# T1 = A * B_inverse\n",
    "# T2 = B_inverse * A\n",
    "B_inv = np.linalg.inv(B)\n",
    "T1 = A @ B_inv\n",
    "T2 = B_inv @ A\n",
    "\n",
    "print(\"Matrix T1 (where T1 * B = A):\")\n",
    "print(T1)\n",
    "print(\"\\nMatrix T2 (where B * T2 = A):\")\n",
    "print(T2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e54d3b37",
   "metadata": {},
   "outputs": [],
   "source": [
    "# STEP 1: Transformation matrix for the camera convention (right-multiplication)\n",
    "# This flips the local Y and Z axes.\n",
    "T_camera = np.array([\n",
    "    [1., 0., 0., 0.],\n",
    "    [0.,-1., 0., 0.],\n",
    "    [0., 0.,-1., 0.],\n",
    "    [0., 0., 0., 1.]\n",
    "])\n",
    "\n",
    "# STEP 2: Transformation matrix for the world system (left-multiplication)\n",
    "# This swaps the world X and Y axes and inverts the world Z axis.\n",
    "T_world = np.array([\n",
    "    [0., 1., 0., 0.],\n",
    "    [1., 0., 0., 0.],\n",
    "    [0., 0.,-1., 0.],\n",
    "    [0., 0., 0., 1.]\n",
    "])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b8336c6e",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Apply the transformations\n",
    "C = B @ T_camera  # Step 1\n",
    "A_transformed = T_world @ C # Step 2\n",
    "\n",
    "# Verify the result\n",
    "print(\"Original A:\\n\", A)\n",
    "print(\"\\nTransformed B:\\n\", A_transformed)\n",
    "print(\"\\nAre they close?\", np.allclose(A, A_transformed))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3908092b",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(A @ T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7a3ddfe0",
   "metadata": {},
   "outputs": [],
   "source": [
    "# rgb_path = \"/mnt/sda/data/3D/scannetpp/39f36da05b/dslr/resized_undistorted_images/DSC00639.JPG\"\n",
    "# img = imread_cv2(rgb_path, cv2.IMREAD_COLOR)\n",
    "# print(img.shape, type(img))\n",
    "# plt.imshow(img)\n",
    "# plt.show()\n",
    "# # (1168, 1752, 3) <class 'numpy.ndarray'>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "384b65ee",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "# Scannet++ depth dataset\n",
    "s = 0\n",
    "# List of file paths\n",
    "scene_dir = \"/mnt/sda/scannetpp_processed/39f36da05b\"\n",
    "file_paths = sorted([f for f in os.listdir(scene_dir) if f.endswith(\"_rgb.jpg\")])\n",
    "cols = 2\n",
    "rows = 2\n",
    "interval = 2\n",
    "# Create a figure with 2 columns and 10 rows,dpi=100\n",
    "fig, axs = plt.subplots(rows, cols, figsize=(12, rows*4))\n",
    "print(len(file_paths))\n",
    "\n",
    "# Iterate over the file paths and display the images\n",
    "for i, file_path in enumerate(file_paths[s:rows*cols*interval//2+s:interval]):\n",
    "    rgb_path = osp.join(scene_dir, file_path)\n",
    "    depth_path = rgb_path.replace(\"_rgb.jpg\", \"_depth.png\")\n",
    "    cam_path = rgb_path.replace(\"_rgb.jpg\", \"_cam.npz\")\n",
    "\n",
    "    img = imread_cv2(rgb_path, cv2.IMREAD_COLOR)\n",
    "    depth = imread_cv2(depth_path, cv2.IMREAD_UNCHANGED)\n",
    "    depth[depth == 65535] = 0  # Handle invalid depth values\n",
    "    depth = depth.astype(np.float32)/1000.0\n",
    "    \n",
    "    cam_file = np.load(cam_path)\n",
    "    desc = f'{depth.min()}, {depth.max()}, {depth.mean()}'\n",
    "    cam_desc = f'{cam_file['pose'][:3, -1]}'\n",
    "    print(cam_file['pose'])\n",
    "    print(cam_file['intrinsics'])\n",
    "\n",
    "\n",
    "    axs[i * 2 // cols , i * 2 % cols ].imshow(img)\n",
    "    axs[i * 2 // cols , i * 2 % cols].axis(\"off\")  # Hide axis ticks\n",
    "    axs[i * 2 // cols , i * 2 % cols].set_title(cam_desc)\n",
    "    axs[i * 2 // cols , i * 2 % cols + 1].imshow(depth)\n",
    "    axs[i * 2 // cols , i * 2 % cols + 1].axis(\"off\")  # Hide axis ticks\n",
    "    axs[i * 2 // cols , i * 2 % cols + 1].set_title(desc)\n",
    "\n",
    "plt.show()\n",
    "print(img.shape)\n",
    "# continious pose\n",
    "# totally correct\n",
    "# 0271889ec0 depth is 0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "69b88453",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Scannet++ depth dataset\n",
    "s = 0\n",
    "# List of file paths\n",
    "scene_dir = \"/home/liucong/data/3d/scannetpp_processed2/0271889ec0\"\n",
    "file_paths = sorted([f for f in os.listdir(scene_dir) if f.endswith(\"_rgb.jpg\")])\n",
    "cols = 2\n",
    "rows = 2\n",
    "interval = 2\n",
    "# Create a figure with 2 columns and 10 rows,dpi=100\n",
    "fig, axs = plt.subplots(rows, cols, figsize=(12, rows*4))\n",
    "print(len(file_paths))\n",
    "\n",
    "# Iterate over the file paths and display the images\n",
    "for i, file_path in enumerate(file_paths[s:rows*cols*interval//2+s:interval]):\n",
    "    rgb_path = osp.join(scene_dir, file_path)\n",
    "    depth_path = rgb_path.replace(\"_rgb.jpg\", \"_depth.png\")\n",
    "    cam_path = rgb_path.replace(\"_rgb.jpg\", \"_cam.npz\")\n",
    "\n",
    "    img = imread_cv2(rgb_path, cv2.IMREAD_COLOR)\n",
    "    depth = imread_cv2(depth_path, cv2.IMREAD_UNCHANGED)\n",
    "    depth[depth == 65535] = 0  # Handle invalid depth values\n",
    "    depth = depth.astype(np.float32)/1000.0\n",
    "    \n",
    "    cam_file = np.load(cam_path)\n",
    "    desc = f'{depth.min()}, {depth.max()}, {depth.mean()}'\n",
    "    cam_desc = f'{cam_file['pose'][:3, -1]}'\n",
    "    print(cam_file['pose'])\n",
    "    print(cam_file['intrinsics'])\n",
    "\n",
    "\n",
    "    axs[i * 2 // cols , i * 2 % cols ].imshow(img)\n",
    "    axs[i * 2 // cols , i * 2 % cols].axis(\"off\")  # Hide axis ticks\n",
    "    axs[i * 2 // cols , i * 2 % cols].set_title(cam_desc)\n",
    "    axs[i * 2 // cols , i * 2 % cols + 1].imshow(depth)\n",
    "    axs[i * 2 // cols , i * 2 % cols + 1].axis(\"off\")  # Hide axis ticks\n",
    "    axs[i * 2 // cols , i * 2 % cols + 1].set_title(desc)\n",
    "\n",
    "plt.show()\n",
    "print(img.shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "709073f5",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "# Scannet++ depth dataset\n",
    "s = 20\n",
    "# List of file paths\n",
    "root = \"/lc/data/3D/scannetpp_depth/nomask/816e996553\"\n",
    "file_paths = sorted([f for f in os.listdir(root) if f.endswith(\".JPG\")])\n",
    "\n",
    "cols = 4\n",
    "rows = 2\n",
    "interval = 1\n",
    "# Create a figure with 2 columns and 10 rows,dpi=100\n",
    "fig, axs = plt.subplots(rows, cols, figsize=(12, rows*2))\n",
    "\n",
    "# Iterate over the file paths and display the images\n",
    "for i, file_path in enumerate(file_paths[s:rows*cols*interval//2+s:interval]):\n",
    "    img = mpimg.imread(osp.join(root, file_path))\n",
    "    depth = mpimg.imread(osp.join(root, file_path.replace(\"rgb.JPG\",\"depth.png\")))\n",
    "    axs[i * 2 // cols , i * 2 % cols ].imshow(img)\n",
    "    axs[i * 2 // cols , i * 2 % cols].axis(\"off\")  # Hide axis ticks\n",
    "    axs[i * 2 // cols , i * 2 % cols].set_title(file_path.split('_')[0]+f\"_{i}\")\n",
    "    axs[i * 2 // cols , i * 2 % cols + 1].imshow(depth)\n",
    "    axs[i * 2 // cols , i * 2 % cols + 1].axis(\"off\")  # Hide axis ticks\n",
    "    axs[i * 2 // cols , i * 2 % cols + 1].set_title(file_path.split('_')[0]+\"_depth\")\n",
    "\n",
    "plt.show()\n",
    "# continious pose"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2616181c",
   "metadata": {},
   "outputs": [],
   "source": [
    "file_path = file_paths[0]\n",
    "img = mpimg.imread(osp.join(root, file_path))\n",
    "print(img.shape)\n",
    "cam_f = file_path.split('_')[0] + '_cam.npz'\n",
    "cam = np.load(osp.join(root, cam_f))\n",
    "print(\"intrinsics\", cam['intrinsics'].shape)\n",
    "print(cam['intrinsics'])\n",
    "print(\"pose\", cam['pose'].shape)\n",
    "print(cam['pose'])\n",
    "\n",
    "# depth = imageio.imread(osp.join(root, file_path.replace(\"rgb.JPG\",\"depth.png\")))\n",
    "depth = mpimg.imread(osp.join(root, file_path.replace(\"rgb.JPG\",\"depth.png\")))\n",
    "print(depth.shape, depth.dtype) \n",
    "print(describe(depth, axis=None))\n",
    "print(describe(depth*65535, axis=None))\n",
    "# import cv2\n",
    "# cv_depth = cv2.imread(osp.join(root, file_path.replace(\"rgb.JPG\",\"depth.png\")), cv2.IMREAD_UNCHANGED).astype(np.float32)/1000.0\n",
    "# print(describe(cv_depth, axis=None))\n",
    "# print(describe(cv_depth/65535, axis=None))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "416714dd",
   "metadata": {},
   "source": [
    "(512, 768, 3)\n",
    "intrinsics (3, 3)\n",
    "[[203.41861225   0.         384.        ]\n",
    " [  0.         203.63734231 256.        ]\n",
    " [  0.           0.           1.        ]]\n",
    "pose (4, 4)\n",
    "[[-0.49830366 -0.12977784  0.85723461  2.56764775]\n",
    " [-0.86662927  0.10356803 -0.48808541  3.52615311]\n",
    " [-0.02543943 -0.98611935 -0.1640776   1.54122865]\n",
    " [ 0.          0.          0.          1.        ]]\n",
    "(512, 768) float32"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "vggt",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.11"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
